/*! 
 * Implementation file for a spring force generator.
 *
 * \author Devis Rossini
 * \date 19 March 2012
 */ 
#include "PhxSpringForceGenerator.h"

using namespace phx;

Spring::Spring(const Vector3& localConnectionPoint, RigidBody* otherBody, const Vector3& otherConnectionPoint, real springConstant, real restLength)
	:	mConnectionPoint(localConnectionPoint),
		mOtherConnectionPoint(otherConnectionPoint),
		mOtherBody(otherBody),
		mSpringConstant(springConstant),
		mRestLength(restLength) { }

void Spring::UpdateForce(RigidBody* body, real duration)
{
	// Calculate the two ends in world space
	Vector3 lws = body->GetPointInWorldSpace(mConnectionPoint);
	Vector3 ows = mOtherBody->GetPointInWorldSpace(mOtherConnectionPoint);

	// Calculate the vector of the spring
	Vector3 force = lws - ows;

	// Calculate the magnitude of the force
	real magnitude = force.Magnitude();
	magnitude = real_abs(magnitude - mRestLength);
	magnitude *= mSpringConstant;

	// Calculate the final force and apply it
	force.Normalise();
	force *= -magnitude;
	body->AddForceAtPoint(force, lws);
}